#include "pointcloud_fusion.h"
#include <pcl/registration/icp.h>

PointCloudFusion::PointCloudFusion(){
    
}

// 设置传感器参数数据
void PointCloudFusion::setParams(const SensorParams& sensor_params){
    
}

// 添加点云数据
void PointCloudFusion::addPointCloudData(pcl::PointCloud<pcl::PointXYZ>::Ptr data_ptr){

}

// ICP 配准
void PointCloudFusion::ICP(pcl::PointCloud<pcl::PointXYZ>::Ptr& data_input, pcl::PointCloud<pcl::PointXYZ>::Ptr& data_output){
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(data_input);
    icp.setInputTarget(data_output);

    // 创建一个 pcl::PointCloud<pcl::PointXYZ>实例 Final 对象,存储配准变换后的源点云,
    // 应用 ICP 算法后, IterativeClosestPoint 能够保存结果点云集,如果这两个点云匹配正确的话
    // （即仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云）,那么 icp. hasConverged()= 1 (true),
    // 然后会输出最终变换矩阵的匹配分数和变换矩阵等信息。
    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);

    // std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
    if (icp.hasConverged() && (icp.getFitnessScore() > 0.3)){
        pcl::transformPointCloud(*data_input, *data_input, icp.getFinalTransformation());
    }    
}



// 获取融合后的数据
pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloudFusion::getFusionData(){
    
}